/*******************************************************************************
* This file implements the Direction Cosine Matrix (DCM) to calculate the 
* attitude base on the data from the IMU.
*
* Author: Kong Wai Weng @ Cytron Technologies Sdn. Bhd.
*******************************************************************************/



#include "dcm.h"



/*******************************************************************************
* PRIVATE CONSTANT DEFINITION                                                  *
*******************************************************************************/

// Period for the DCM loop (5ms - 200Hz).
#define DCM_DT              0.005f

// The gain for drift correction.
#define KP_ACC              0.2f		// Derived from APM (0.0014 * 418.0).
#define KI_ACC              0.01f		// Derived from APM (0.0000003 * 418.0 / 0.02).

#define KP_MAG              0.2f		// Derived from APM.
#define KI_MAG              0.01f		// Derived from APM (0.00004 / 0.02).

//#define KP_ACC				0.5852f		// Derived from APM (0.0014 * 418.0).
//#define KI_ACC				0.00627f	// Derived from APM (0.0000003 * 418.0 / 0.02).
//
//#define KP_MAG				0.8f		// Derived from APM.
//#define KI_MAG				0.002f		// Derived from APM (0.00004 / 0.02).

// Do not update I for yaw axis if spin too fast (rad/s).
#define SPIN_RATE_LIMIT     (20 * DEG_TO_RAD)

// When sampling for gyro offset, the sample must not more than this value.
#define GYRO_VARIANCE       (0.03 * DEG_TO_RAD)


/*******************************************************************************
* PRIVATE GLOBAL VARIABLES                                                     *
*******************************************************************************/

// The Direction Cosine Matrix.
// Content of the matrix:
// { cos(yaw)cos(pitch)    cos(yaw)sin(pitch)sin(roll) - sin(yaw)cos(roll)    cos(yaw)sin(pitch)cos(roll) + sin(yaw)sin(roll) }
// { sin(yaw)cos(pitch)    sin(yaw)sin(pitch)sin(roll) + cos(yaw)cos(roll)    sin(yaw)sin(pitch)cos(roll) - cos(yaw)sin(roll) }
// { -sin(pitch)           cos(pitch)sin(roll)                                cos(pitch)cos(roll)                             }
static volatile float prv_fDcm[3][3] = {{1.0, 0.0, 0.0},
                                        {0.0, 1.0, 0.0},
                                        {0.0, 0.0, 1.0}};

// Vector for corrected angular rate.
static volatile float prv_fRateVector[3] = {0.0};

// Angular rate vector for centripetal force correction.
static volatile float prv_fRateVectorCentripetalCorrection[3] = {0.0};

// Vector for proportional angular rate correction.
static volatile float prv_fRateCorrectionP[3] = {0.0};

// Vector for integral angular rate correction.
static volatile float prv_fRateCorrectionI[3] = {0.0};

// The attitude of the system.
static volatile struct ATTITUDE prv_xAttitude = {0.0};
static volatile struct ATTITUDE prv_xAttitudeTemp = {0.0};

// Sensors reading.
static volatile float prv_fAccVector[3] = {0.0};
static volatile float prv_fGyroVector[3] = {0.0};
static volatile float prv_fMagVector[3] = {0.0};



/*******************************************************************************
* PRIVATE FUNCTION PROTOTYPES                                                  *
*******************************************************************************/

static void prv_vInitDcm(void);
static void prv_vDcmPredict(void);
static void prv_vDcmCorrectDrift(void);
static void prv_vMatrixMultiply3x3(float fResult[3][3], const float fMatrix1[3][3], const float fMatrix2[3][3]);
static void prv_vVectorCross3(float fResult[3], const float fVector1[3], const float fVector2[3]);
static void prv_vNormalize(void);
static void prv_vReadSensors(void);



/*******************************************************************************
* PRIVATE FUNCTION: prv_vInitDcm
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Initialize the DCM to the value sampled during start up.
* This function must be called after the sensors are initialized.
*
*******************************************************************************/
static void prv_vInitDcm(void)
{
    unsigned int i;
    float fGyroVectorSum[3] = {0.0};
    float fGyroVectorAverage1[3] = {0.0};
    float fGyroVectorAverage2[3] = {0.0};
    float fGyroVectorDiff[3] = {0.0};

    

    do {
        // Get the average value of the sensors.
        fGyroVectorSum[0] = 0.0f;
        fGyroVectorSum[1] = 0.0f;
        fGyroVectorSum[2] = 0.0f;
        for (i = 0; i < 200; i++) {
            // Read from the sensors.
            prv_vReadSensors();

            // Accumulate the gyro reading.
            fGyroVectorSum[0] += prv_fGyroVector[0];
            fGyroVectorSum[1] += prv_fGyroVector[1];
            fGyroVectorSum[2] += prv_fGyroVector[2];

            // Delay for 10ms.
            vTaskDelay(configTICK_RATE_HZ / 100);
        }

        // Save gyro average reading as first average reading.
        for (i = 0; i < 3; i++) {
            fGyroVectorAverage1[i] = fGyroVectorSum[i] / 200.0;
        }



        // Delay for 500ms.
        vTaskDelay(configTICK_RATE_HZ / 2);



        // Get the average value of the sensors.
        fGyroVectorSum[0] = 0.0f;
        fGyroVectorSum[1] = 0.0f;
        fGyroVectorSum[2] = 0.0f;
        for (i = 0; i < 200; i++) {
            // Read from the sensors.
            prv_vReadSensors();

            // Accumulate the gyro reading.
            fGyroVectorSum[0] += prv_fGyroVector[0];
            fGyroVectorSum[1] += prv_fGyroVector[1];
            fGyroVectorSum[2] += prv_fGyroVector[2];

            // Delay for 10ms.
            vTaskDelay(configTICK_RATE_HZ / 100);
        }

        // Save gyro average reading as first average reading.
        for (i = 0; i < 3; i++) {
            fGyroVectorAverage2[i] = fGyroVectorSum[i] / 200.0;
        }



        for (i = 0; i < 3; i++) {
            fGyroVectorDiff[i] = fGyroVectorAverage2[i] - fGyroVectorAverage1[i];
            if (fGyroVectorDiff[i] < 0) {
                fGyroVectorDiff[i] = -fGyroVectorDiff[i];
            }
        }
    } while ((fGyroVectorDiff[0] > GYRO_VARIANCE) || (fGyroVectorDiff[1] > GYRO_VARIANCE) || (fGyroVectorDiff[2] > GYRO_VARIANCE));

    // Use the gyro average reading as the initial value for bias.
    for (i = 0; i < 3; i++) {
        prv_fRateCorrectionI[i] = -fGyroVectorAverage2[i];
    }


    // Calculate the pitch, roll and yaw from the accelerometer and magnetometer.
    float fPitch = fGetPitchAcc();
    float fRoll = fGetRollAcc();
    float fYaw = fGetYawMag(fPitch, fRoll);

    // Calculate and populate the DCM.
    float fCosRoll = cosf(fRoll);
    float fSinRoll = sinf(fRoll);
    float fCosPitch = cosf(fPitch);
    float fSinPitch = sinf(fPitch);
    float fCosYaw = cosf(fYaw);
    float fSinYaw = sinf(fYaw);
    
    prv_fDcm[0][0] = fCosYaw*fCosPitch;
    prv_fDcm[0][1] = (fCosYaw*fSinPitch*fSinRoll) - (fSinYaw*fCosRoll);
    prv_fDcm[0][2] = (fCosYaw*fSinPitch*fCosRoll) + (fSinYaw*fSinRoll);
    prv_fDcm[1][0] = fSinYaw*fCosPitch;
    prv_fDcm[1][1] = (fSinYaw*fSinPitch*fSinRoll) + (fCosYaw*fCosRoll);
    prv_fDcm[1][2] = (fSinYaw*fSinPitch*fCosRoll) - (fCosYaw*fSinRoll);
    prv_fDcm[2][0] = -fSinPitch;
    prv_fDcm[2][1] = fCosPitch*fSinRoll;
    prv_fDcm[3][2] = fCosPitch*fCosRoll;

}



/*******************************************************************************
* PUBLIC FUNCTION: fGetRollAcc
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ The roll angle.
* ~ Unit = radian.
*
* DESCRIPTIONS:
* Get the uncompensated roll angle from accelerometer.
* Need to update the sensors reading before calling this function.
*
*******************************************************************************/
float fGetRollAcc(void)
{
    return atan2f(prv_fAccVector[1], prv_fAccVector[2]);
}



/*******************************************************************************
* PUBLIC FUNCTION: fGetPitchAcc
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ The pitch angle.
* ~ Unit = radian.
*
* DESCRIPTIONS:
* Get the uncompensated pitch angle from accelerometer.
* Need to update the sensors reading before calling this function.
*
*******************************************************************************/
float fGetPitchAcc(void)
{
    return -atan2f( prv_fAccVector[0], sqrtf( (prv_fAccVector[1] * prv_fAccVector[1]) +
                                              (prv_fAccVector[2] * prv_fAccVector[2]) )
                  );
}



/*******************************************************************************
* PUBLIC FUNCTION: fGetYawMag
*
* PARAMETERS:
* ~ fPitch  - Pitch angle in rad/s.
* ~ fRoll   - Roll angle in rad/s.
*
* RETURN:
* ~ The yaw angle.
* ~ Unit = radian.
*
* DESCRIPTIONS:
* Get the uncompensated yaw angle from magnetometer.
* Need to update the sensors reading before calling this function.
*
*******************************************************************************/
float fGetYawMag(float fPitch, float fRoll)
{
    float fCosRoll = cosf(fRoll);
    float fSinRoll = sinf(fRoll);
    float fCosPitch = cosf(fPitch);
    float fSinPitch = sinf(fPitch);

    // Tilt compensated Magnetic field X.
    float fTiltCompensatedMagX = (prv_fMagVector[0] * fCosPitch) +
                                 (prv_fMagVector[1] * fSinRoll * fSinPitch) +
                                 (prv_fMagVector[2] * fCosRoll * fSinPitch);

    // Tilt compensated Magnetic field Y.
    float fTiltCompensatedMagY = (prv_fMagVector[1] * fCosRoll) - (prv_fMagVector[2] * fSinRoll);

    // Calculate the magnetic heading.
    float fMagHeading = atan2f(-fTiltCompensatedMagY, fTiltCompensatedMagX);

    // Correct the magnetic declination.
    fMagHeading += MAG_DECL;

    // Make sure it's within 0 - 2PI.
    CONVERT_0_2PI(fMagHeading);

    return fMagHeading;
}



/*******************************************************************************
* PUBLIC FUNCTION: xGetAttitude
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ The pitch, roll and yaw angle.
* ~ Unit = radian.
*
* DESCRIPTIONS:
* Get the pitch, roll and yaw angle of the system.
*
*******************************************************************************/
struct ATTITUDE xGetAttitude(void)
{
	struct ATTITUDE xBuffer;
	
	// We don't want to switch the context while we are reading the attitude.
	portENTER_CRITICAL();
	xBuffer = prv_xAttitude;
	portEXIT_CRITICAL();
	
	return xBuffer;
}	



/*******************************************************************************
* PUBLIC FUNCTION: fGetDcm22
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ The value in DCM[2][2].
*
* DESCRIPTIONS:
* Get the value of DCM[2][2].
*
*******************************************************************************/
float fGetDcm22(void)
{
	float fBuffer;
	
	// We don't want to switch the context while we are reading the attitude.
	portENTER_CRITICAL();
	fBuffer = prv_fDcm[2][2];
	portEXIT_CRITICAL();
	
	return fBuffer;
}	



/*******************************************************************************
* TASK: taskDcmLoop
*
* DESCRIPTIONS:
* This task is the main loop of the DCM and will get executed every 5ms (200Hz).
*
*******************************************************************************/
portTASK_FUNCTION(taskDcmLoop, pvParameters)
{
	unsigned int uiLoopCount = 0;
	
	
	// Initialize DCM.
	prv_vInitDcm();
	
	// We need to initialize xLastFlashTime prior to the first call to vTaskDelayUntil().
	portTickType xLastFlashTime = xTaskGetTickCount();
	
	while (1) {
		// Indicate that the DCM is ready when > 10s.
		if (++uiLoopCount > (10 * 200)) {
			xSystemState.bDcmReady = 1;
		}	
		
		
		
		// Read from the sensors.
		prv_vReadSensors();
		
		// Predict next state of DCM.
		prv_vDcmPredict();
		
		// Normalize the DCM.
		prv_vNormalize();
		
		// Correct the gyro drift.
		prv_vDcmCorrectDrift();
		
		
		
		// Convert the DCM to Euler angle.
		prv_xAttitudeTemp.fPitch = asinf(-prv_fDcm[2][0]);
		prv_xAttitudeTemp.fRoll = atan2f(prv_fDcm[2][1], prv_fDcm[2][2]);
		prv_xAttitudeTemp.fYaw = atan2f(prv_fDcm[1][0], prv_fDcm[0][0]);
		
		// Convert the yaw angle to 0 - 2PI.
		CONVERT_0_2PI(prv_xAttitudeTemp.fYaw);
		
		// Calculate the actual rate.
		prv_xAttitudeTemp.fRateX = prv_fRateVectorCentripetalCorrection[0];
		prv_xAttitudeTemp.fRateY = prv_fRateVectorCentripetalCorrection[1];
		prv_xAttitudeTemp.fRateZ = prv_fRateVectorCentripetalCorrection[2];
		
		prv_xAttitudeTemp.fRateBiasX = prv_fRateCorrectionI[0];
		prv_xAttitudeTemp.fRateBiasY = prv_fRateCorrectionI[1];
		prv_xAttitudeTemp.fRateBiasZ = prv_fRateCorrectionI[2];
		
		// Copy the attitude to the private global variable.
		// We don't want to switch the context during this process.
		portENTER_CRITICAL();
		prv_xAttitude = prv_xAttitudeTemp;
		portEXIT_CRITICAL();
		
		
		
		// Record the stack high water mark.
		xStackWaterMark.uiDcmLoop = uxTaskGetStackHighWaterMark(NULL);
		
		// Set the error flag if the water mark is too low.
		if (xStackWaterMark.uiDcmLoop < RTOS_STACK_THRESHOLD) {
			xSystemError.bRtosStackError = 1;
		}	
		
		// Loop every 5ms (200 Hz).
		vTaskDelayUntil(&xLastFlashTime, configTICK_RATE_HZ / 200);
		
	}	// while (1)
}	



/*******************************************************************************
* PRIVATE FUNCTION: prv_vDcmPredict
*
* PARAMETERS:
* ~ void.
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* This is the key routine. It performs a small rotation on the DCM, based on
* the gyro vector and correction.
*
*******************************************************************************/
static void prv_vDcmPredict(void)
{
	unsigned int i, j;
	float fDcmUpdate[3][3];
	float fDcmTemp[3][3];
	
	
	
	// Add the proportional and integral correction vector.
	for (i = 0; i < 3; i++) {
		prv_fRateVectorCentripetalCorrection[i] = prv_fGyroVector[i] + prv_fRateCorrectionI[i];
		prv_fRateVector[i] = prv_fRateVectorCentripetalCorrection[i] + prv_fRateCorrectionP[i];
	}
	
	
	
	// Compute the update matrix.
	fDcmUpdate[0][0] = 1.0;
	fDcmUpdate[0][1] = -prv_fRateVector[2] * DCM_DT;	// -Z
	fDcmUpdate[0][2] = prv_fRateVector[1] * DCM_DT;		// Y
	fDcmUpdate[1][0] = prv_fRateVector[2] * DCM_DT;		// Z
	fDcmUpdate[1][1] = 1.0;
	fDcmUpdate[1][2] = -prv_fRateVector[0] * DCM_DT;	// -X
	fDcmUpdate[2][0] = -prv_fRateVector[1] * DCM_DT;	// -Y
	fDcmUpdate[2][1] = prv_fRateVector[0] * DCM_DT;		// X
	fDcmUpdate[2][2] = 1.0;
	
	// Multiply the update matrix with the DCM.
	prv_vMatrixMultiply3x3(fDcmTemp, (const float (*)[3])prv_fDcm, (const float (*)[3])fDcmUpdate);
	
	// Put the result into the DCM.
	for (i = 0; i < 3; i++) {
		for (j = 0; j < 3; j++) {
			prv_fDcm[i][j] = fDcmTemp[i][j];
		}	
	}		
}



/*******************************************************************************
* PRIVATE FUNCTION: prv_vDcmCorrectDrift
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* This function correct the bias of the gyro by using the data from the
* accelerometer and magnetometer.
*
*******************************************************************************/
static void prv_vDcmCorrectDrift(void)
{
	unsigned int i;
	float fRollPitchError[3];
	float fAccMagnitude;
	float fAccWeight = 1.0f;
	float fSpeed;



        // Calculate the spin rate.
        float fSpinRate = sqrtf(    (prv_fRateVectorCentripetalCorrection[0] * prv_fRateVectorCentripetalCorrection[0]) +
                                    (prv_fRateVectorCentripetalCorrection[1] * prv_fRateVectorCentripetalCorrection[1]) +
                                    (prv_fRateVectorCentripetalCorrection[2] * prv_fRateVectorCentripetalCorrection[2])    );
	
	// Determine the gain base on spin rate.
        float fSpinRateGain;
        if (fSpinRate < (50 * DEG_TO_RAD)) {
            fSpinRateGain = 1.0f;
        }
        else if (fSpinRate > (500 * DEG_TO_RAD)) {
            fSpinRateGain = 10.0f;
        }
        else {
            fSpinRateGain = fSpinRate / (50 * DEG_TO_RAD);
        }



	// #####################################################################
	// Correct pitch and roll drift base on acccelerometer.
	// #####################################################################
	
	// Get the GPS data.
	struct GPS_DATA xGpsData = xGetGpsData();
	
	// Make sure the GPS has valid signal before we use the speed info.
	if ((xGpsData.ucFixStatus != 0) && (xGpsData.fGroundSpeed > 3.0f)) {
		fSpeed = xGpsData.fGroundSpeed;
		
		// Remove the centrifugal acceleration.
		prv_fAccVector[1] += prv_fRateVectorCentripetalCorrection[2] * fSpeed / GRAVITY;
		prv_fAccVector[2] -= prv_fRateVectorCentripetalCorrection[1] * fSpeed / GRAVITY;
	}	
	
	
	
	// Calculate the magnitude of the acceleration vector.
	fAccMagnitude = sqrtf((prv_fAccVector[0] * prv_fAccVector[0]) +
						  (prv_fAccVector[1] * prv_fAccVector[1]) +
						  (prv_fAccVector[2] * prv_fAccVector[2]));
	
	// Dynamic weighting of accelerometer info (reliability filter).
	// Weight for accelerometer info (<0.66G = 0.0, 1G = 1.0 , >1.33G = 0.0).
	fAccWeight = 1.0 - (3.0 * fabsf(1.0 - fAccMagnitude));
	if (fAccWeight < 0.0) {
		fAccWeight = 0.0;
	}



	
//	// Calculate the Z acceleration from X and Y axis.
//	// This reduces the noise.
//	float fAccZSquared = 1.0f - ((prv_fAccVector[0] * prv_fAccVector[0]) + (prv_fAccVector[1] * prv_fAccVector[1]));
//
//	// Make sure the AccZ^2 is positive.
//	if (fAccZSquared >= 0) {
//
//		// Get the sign base on the actual Z Acc reading.
//		if (prv_fAccVector[2] >= 0) {
//			prv_fAccVector[2] = sqrtf(fAccZSquared);
//		}
//		else {
//			prv_fAccVector[2] = -sqrtf(fAccZSquared);
//		}
	
	
		// Cross product (fAccVector X prv_fDcm[2]) to get the error.
		prv_vVectorCross3(fRollPitchError, (const float*)prv_fAccVector, (const float*)prv_fDcm[2]);
		
		
//		// Calculate the magnitude of the error vector.
//		float fErrorMagnitude = sqrtf((fRollPitchError[0] * fRollPitchError[0]) +
//							  		  (fRollPitchError[1] * fRollPitchError[1]) +
//							  		  (fRollPitchError[2] * fRollPitchError[2]));
//
		
		
		for (i = 0; i < 3; i++) {
			
//			// Compute the proportional correction vector.
//			if (xSystemState.bDcmReady == 0) {
//				prv_fRateCorrectionP[i] = fRollPitchError[i] * fAccWeight;
//			}
//			else {
//				// Limit the maximum error.
//				if (fErrorMagnitude > 0.2f) {
//					fRollPitchError[i] *= (0.2f / fErrorMagnitude);
//				}	
				
				prv_fRateCorrectionP[i] = fRollPitchError[i] * KP_ACC * fAccWeight * fSpinRateGain;
//			}
			
			
			// Limit the error for integral.
			fRollPitchError[i] = LIMIT(fRollPitchError[i], -0.03, 0.03);
			
			// Compute the integral correction vector.
                        if (fSpinRate < SPIN_RATE_LIMIT) {
                            prv_fRateCorrectionI[i] += fRollPitchError[i] * KI_ACC * DCM_DT * fAccWeight;
                        }
			
		}	
		
//	}	// if (fAccZSquared >= 0)
//
//	else {
//		for (i = 0; i < 3; i++) {
//			prv_fRateCorrectionP[i] = 0.0f;
//		}
//	}
	
	
	// #####################################################################
	// Calculate the magnetic heading.
	// #####################################################################

        float fPitch = asinf(-prv_fDcm[2][0]);
	float fRoll = atan2f(prv_fDcm[2][1], prv_fDcm[2][2]);

	prv_xAttitudeTemp.fMagHeading = fGetYawMag(fPitch, fRoll);


        
	// #####################################################################
	// Correct the yaw drift base on magnetic or GPS heading.
	// #####################################################################
	
	float fHeadingX;
	float fHeadingY;
	
	// If GPS data is valid and the speed is > 3m/s (10.8km/h), use the GPS heading.
	if ((xGpsData.ucFixStatus != 0) && (xGpsData.fGroundSpeed > 3.0f)) {
		fHeadingX = cosf(xGpsData.fGroundCourse * DEG_TO_RAD);
		fHeadingY = sinf(xGpsData.fGroundCourse * DEG_TO_RAD);
	}
	// Else, use the magnetic bearing.
	else {
		fHeadingX = cosf(prv_xAttitudeTemp.fMagHeading);
		fHeadingY = sinf(prv_xAttitudeTemp.fMagHeading);
	}	
	
	
	
	// Calculate yaw error along the Earth frame.
    float fEarthYawError = (prv_fDcm[0][0] * fHeadingY) - (prv_fDcm[1][0] * fHeadingX);
    
    // Limit the error to +-PI.
    CONVERT_NEGPI_PI(fEarthYawError);
    
    // Convert the error to the body frame.
    float fBodyYawError[3];
    for (i = 0; i < 3; i++) {
		fBodyYawError[i] = prv_fDcm[2][i] * fEarthYawError;
	} 
	
	
	for (i = 0; i < 3; i++) {
		// Compute the proportional correction vector.
//		if (xSystemState.bDcmReady == 0) {
//			prv_fRateCorrectionP[i] += fBodyYawError[i];
//		}
//		else {
			prv_fRateCorrectionP[i] += fBodyYawError[i] * KP_MAG * fSpinRateGain;
//		}
	}
	
	// Limit the error for integral.
	fBodyYawError[2] = LIMIT(fBodyYawError[2], -0.03, 0.03);
	
	// Compute the integral correction vector only for Z gyro.
        if (fSpinRate < SPIN_RATE_LIMIT) {
            prv_fRateCorrectionI[2] += fBodyYawError[2] * KI_MAG * DCM_DT;
        }
	
	
	
	// #####################################################################
	// #####################################################################
	
//	
//	
//	// Here we will place a limit on the integrator so that the integrator cannot
//	// ever exceed half the saturation limit of the gyros.
//	float fIntegratorMagnitude = sqrtf((prv_fRateCorrectionI[0] * prv_fRateCorrectionI[0]) +
//									   (prv_fRateCorrectionI[1] * prv_fRateCorrectionI[1]) +
//									   (prv_fRateCorrectionI[2] * prv_fRateCorrectionI[2]));
//	
//	if (fIntegratorMagnitude > (300.0 * DEG_TO_RAD)) {
//		for (i = 0; i < 3; i++) {
//			prv_fRateCorrectionI[i] *= 0.5 * (300.0 * DEG_TO_RAD) / fIntegratorMagnitude;
//		}
//	}	
}



/*******************************************************************************
* PRIVATE FUNCTION: prv_vMatrixMultiply3x3
*
* PARAMETERS:
* ~ fResult			- Array to store the result.
* ~ fMatrix1		- First matrix.
* ~ fMatrix2		- Second matrix.
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Function for 3X3 matrix multiplication.
*
*******************************************************************************/
static void prv_vMatrixMultiply3x3(float fResult[3][3], const float fMatrix1[3][3], const float fMatrix2[3][3])
{
	unsigned int i, j, k;
	float fBuffer[3];
	
	for(i = 0; i < 3; i++) {
		
		for(j = 0; j < 3; j++) {
			
			for(k = 0; k < 3; k++) {
				fBuffer[k] = fMatrix1[i][k] * fMatrix2[k][j];
			}
			
			fResult[i][j] = fBuffer[0] + fBuffer[1] + fBuffer[2];
			
		}
	}	
}



/*******************************************************************************
* PRIVATE FUNCTION: prv_vVectorCross3
*
* PARAMETERS:
* ~ fResult			- Array to store the result.
* ~ fVector1		- First matrix.
* ~ fVector2		- Second matrix.
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Function for 1X3 Vector Cross Product.
*
*******************************************************************************/
static void prv_vVectorCross3(float fResult[3], const float fVector1[3], const float fVector2[3])
{
	fResult[0] = (fVector1[1] * fVector2[2]) - (fVector1[2] * fVector2[1]);
	fResult[1] = (fVector1[2] * fVector2[0]) - (fVector1[0] * fVector2[2]);
	fResult[2] = (fVector1[0] * fVector2[1]) - (fVector1[1] * fVector2[0]);
}



/*******************************************************************************
* PRIVATE FUNCTION: prv_vNormalize
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* This is the routine that maintains the orthogonality of the direction cosine
* matrix, which is expressed by the identity relationship that the cosine
* matrix multiplied by its transpose should equal the identity matrix.
* Small adjustments are made at each time step to assure orthogonality.
*
*******************************************************************************/
static void prv_vNormalize(void)

{
	unsigned int i;
	float fDcmTemp[3][3];
	float fError;
	float fScale;
	
	
	
	// Compute 1/2 of the dot product between row 1 and 2.
	fError =  0.5 * ((prv_fDcm[0][0] * prv_fDcm[1][0]) +
					 (prv_fDcm[0][1] * prv_fDcm[1][1]) +
					 (prv_fDcm[0][2] * prv_fDcm[1][2]));
	
	// Update the first 2 rows to make it closer to orthogonal.
	for (i = 0; i < 3; i++) {
		// Do the calculation in the temporary buffer.
		fDcmTemp[0][i] = prv_fDcm[0][i] - (fError * prv_fDcm[1][i]);
		fDcmTemp[1][i] = prv_fDcm[1][i] - (fError * prv_fDcm[0][i]);
	}	
	
	//	Use the cross product of the first 2 rows to get the 3rd row.
	fDcmTemp[2][0] = (fDcmTemp[0][1] * fDcmTemp[1][2]) - (fDcmTemp[0][2] * fDcmTemp[1][1]);
	fDcmTemp[2][1] = (fDcmTemp[0][2] * fDcmTemp[1][0]) - (fDcmTemp[0][0] * fDcmTemp[1][2]);
	fDcmTemp[2][2] = (fDcmTemp[0][0] * fDcmTemp[1][1]) - (fDcmTemp[0][1] * fDcmTemp[1][0]);
	
	
	
	//	Rescale row 1 so that the magnitude is as close as possible to 1.	
	fScale = sqrtf((fDcmTemp[0][0] * fDcmTemp[0][0]) +
				   (fDcmTemp[0][1] * fDcmTemp[0][1]) +
				   (fDcmTemp[0][2] * fDcmTemp[0][2]));
	
	prv_fDcm[0][0] = fDcmTemp[0][0] / fScale;
	prv_fDcm[0][1] = fDcmTemp[0][1] / fScale;
	prv_fDcm[0][2] = fDcmTemp[0][2] / fScale;
	
	
	
	//	Rescale row 2 so that the magnitude is as close as possible to 1.	
	fScale = sqrtf((fDcmTemp[1][0] * fDcmTemp[1][0]) +
				   (fDcmTemp[1][1] * fDcmTemp[1][1]) +
				   (fDcmTemp[1][2] * fDcmTemp[1][2]));
	
	prv_fDcm[1][0] = fDcmTemp[1][0] / fScale;
	prv_fDcm[1][1] = fDcmTemp[1][1] / fScale;
	prv_fDcm[1][2] = fDcmTemp[1][2] / fScale;
	
	
	
	//	Rescale row 3 so that the magnitude is as close as possible to 1.	
	fScale = sqrtf((fDcmTemp[2][0] * fDcmTemp[2][0]) +
				   (fDcmTemp[2][1] * fDcmTemp[2][1]) +
				   (fDcmTemp[2][2] * fDcmTemp[2][2]));
	
	prv_fDcm[2][0] = fDcmTemp[2][0] / fScale;
	prv_fDcm[2][1] = fDcmTemp[2][1] / fScale;
	prv_fDcm[2][2] = fDcmTemp[2][2] / fScale;
}



/*******************************************************************************
* PRIVATE FUNCTION: prv_vReadSensors
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Read from accelerometer, gyro and magnetometer.
* The reading will be stored in the corresponding vector.
*
*******************************************************************************/
static void prv_vReadSensors(void)
{
	// Read from accelerometer, gyro and magnetometer.
	struct ACC_DATA xAccData = xReadAcc();
	struct GYRO_DATA xGyroData = xReadGyro();
	struct MAG_DATA xMagData = xReadMag();
	
	
	
	// Put the reading for accelerometer, gyro and magnetometer into the vector.
	// Low pass filter the data.
	// For accelerometer data, we need to invert the direction to be used in DCM.
	prv_fAccVector[0] = -xAccData.fAccX;
	prv_fAccVector[1] = -xAccData.fAccY;
	prv_fAccVector[2] = -xAccData.fAccZ;
	
//	prv_fAccVector[0] = (prv_fAccVector[0] * 0.99f) + (-xAccData.fAccX * 0.01f);
//	prv_fAccVector[1] = (prv_fAccVector[1] * 0.99f) + (-xAccData.fAccY * 0.01f);
//	prv_fAccVector[2] = (prv_fAccVector[2] * 0.99f) + (-xAccData.fAccZ * 0.01f);
	
//	prv_fAccVector[0] = 0.0;
//	prv_fAccVector[1] = 0.0;
//	prv_fAccVector[2] = 1.0;
	
	prv_fGyroVector[0] = xGyroData.fGyroX;
	prv_fGyroVector[1] = xGyroData.fGyroY;
	prv_fGyroVector[2] = xGyroData.fGyroZ;
	
//	prv_fGyroVector[0] = 0.0;
//	prv_fGyroVector[1] = 0.0;
//	prv_fGyroVector[2] = 0.0;
	
//	prv_fGyroVector[0] = (prv_fGyroVector[0] * 0.7) + (xGyroData.fGyroX * 0.3);
//	prv_fGyroVector[1] = (prv_fGyroVector[1] * 0.7) + (xGyroData.fGyroY * 0.3);
//	prv_fGyroVector[2] = (prv_fGyroVector[2] * 0.7) + (xGyroData.fGyroZ * 0.3);
	
	prv_fMagVector[0] = xMagData.fMagX;
	prv_fMagVector[1] = xMagData.fMagY;
	prv_fMagVector[2] = xMagData.fMagZ;
	
//	prv_fMagVector[0] = (prv_fMagVector[0] * 0.9) + (xMagData.fMagX * 0.1);
//	prv_fMagVector[1] = (prv_fMagVector[1] * 0.9) + (xMagData.fMagY * 0.1);
//	prv_fMagVector[2] = (prv_fMagVector[2] * 0.9) + (xMagData.fMagZ * 0.1);
}	
